package roboticsapp;

import java.awt.Color;
import java.awt.Graphics;
import java.lang.Math;
import java.util.ArrayList;

public class Robot1 {
    private Point a1;        //Axis 1 Location
    private Point a2;        //Axis 2 Location
    private Point a3;        //Axis 3 Location
    private Point pb;        //Paint Brush Location
    
    private double theta1;   //Axis 1 Angle
    private double theta2;   //Axis 2 Angle
    private double theta3;   //Axis 3 Angle
    
    private double l1 = 150; //Length of Link 1
    private double l2 = 100; //Length of Link 2
    private double l3 = 75;  //Length of Link 3
    
    private boolean paint=false;
    
    private ArrayList<Arc> ovals = new ArrayList<Arc>();
    private ArrayList<Point> points = new ArrayList<Point>();
    
    Robot1(){
        
        a1 = new Point(350, 480);
        a2 = new Point(350, 480 - l1);
        //a1=new Point(350,680);   //Draws out of the screen on my computer
        //a2=new Point(350,680-l1);
        a3=new Point(350, a2.y-l2);
        pb=new Point(350, a3.y-l3);
        theta1 = 0;
        theta2 = 0;
        theta3 = 0;
        
    }
    
    public void togglePaint()
    {
        paint=!paint;
        System.out.println("paint is now:" + paint);
    }
    
    public void draw(Graphics g)
    {        
        calc_points_from_angles();
        
        g.setColor(Color.black);
        g.drawLine((int)a1.x, (int)a1.y, (int)a2.x, (int)a2.y);
        g.drawLine((int)a2.x, (int)a2.y, (int)a3.x, (int)a3.y);
        g.drawLine((int)a3.x, (int)a3.y, (int)pb.x, (int)pb.y);
        g.fillOval((int)a1.x-3, (int)a1.y-3,6,6);
        g.fillOval((int)a2.x-3, (int)a2.y-3,6,6);
        g.fillOval((int)a3.x-3, (int)a3.y-3,6,6);
        g.fillOval((int)pb.x-3, (int)pb.y-3,6,6);
        
        g.setColor(Color.red);
        for (int i=0;i<points.size();i++) {
            g.fillOval((int)points.get(i).x-5,(int) points.get(i).y-5, 10, 10);
            //g.drawArc((int)ovals.get(i).x,(int)ovals.get(i).y,(int)ovals.get(i).size,(int)ovals.get(i).size,(int)ovals.get(i).startAngle,(int)ovals.get(i).arcAngle);
        }
    }
    
    /*public void paint(Graphics g)
    {
        g.setColor(Color.red);
        g.fillOval((int)(pb.x - 5), (int)(pb.y - 5), 10, 10);
    }*/
    
    private void calc_points_from_angles()
    {
        //Calculates a1,a2,a3,pb from values of theta and lengths
        System.out.println("Points recalculated based on:");
        System.out.println("theta1: " + Math.toRadians(theta1));
        System.out.println("theta2: " + Math.toRadians(theta2));
        System.out.println("theta3: " + Math.toRadians(theta3));
        
        a2.x = a1.x + (l1 * Math.cos(Math.toRadians(theta1)));
        a2.y = a1.y - (l1 * Math.sin(Math.toRadians(theta1)));
        System.out.println("a1: " + a1);
        System.out.println("a2: " + a2);
        
        a3.x = a2.x + (l2 * Math.cos(Math.toRadians(theta2 + theta1)));
        a3.y = a2.y - (l2 * Math.sin(Math.toRadians(theta2 + theta1)));
        System.out.println("a3: " + a3);
        
        pb.x = a3.x + (l3 * Math.cos(Math.toRadians(theta3 + theta2 + theta1)));
        pb.y = a3.y - (l3 * Math.sin(Math.toRadians(theta3 + theta2 + theta1)));
        System.out.println("pb: " + pb);
    }
    public void redraw (Graphics g, int link, int offset)
    {
        double old_angle=(theta1+theta2+theta3)%360;
        double length=0;
        double x=0;
        //Recalculate angles
        if (link==1) {
            theta1+=offset;
            theta1=theta1%360;
            length=l1;
        }
        else if(link==2) {
            theta2+=offset;
            theta2=theta2%360;
            length=l2;
        }
        else if(link==3) {
            theta3+=offset;
            theta3=theta3%360;
            length=l3;
        }
        //Add painted points if neccessary
        if(paint)
        {
            System.out.println("Added a point");
            //ovals.add(new Arc(a3.x-length,a3.y-length,length*2,old_angle,1));
            points.add(new Point(pb.x,pb.y));
        }
        //Erase old arm
        /*
        g.setColor(Color.white);
        System.out.println("Clearing Link 3");
        g.drawLine((int)a3.x, (int)a3.y, (int)pb.x, (int)pb.y);
        g.fillOval((int)pb.x-3, (int)pb.y-3,6,6);
        if(link<3) {
            System.out.println("Clearing Link 2");
            g.drawLine((int)a2.x, (int)a2.y, (int)a3.x, (int)a3.y);
            g.fillOval((int)a3.x-3, (int)a3.y-3,6,6);
            if(link<2) {
                System.out.println("Clearing Link 3");
                g.drawLine((int)a1.x, (int)a1.y, (int)a2.x, (int)a2.y);
                g.fillOval((int)a2.x-3, (int)a2.y-3,6,6);
            }
        }
        //Draw new arms
        calc_points_from_angles();
        g.setColor(Color.black);
        System.out.println("Drawing Link 3");
        g.drawLine((int)a3.x, (int)a3.y, (int)pb.x, (int)pb.y);
        g.fillOval((int)pb.x-3, (int)pb.y-3,6,6);
        System.out.println("Drawing Link 2");
        g.drawLine((int)a2.x, (int)a2.y, (int)a3.x, (int)a3.y);
        g.fillOval((int)a3.x-3, (int)a3.y-3,6,6);
        if(link<3) {
            System.out.println("Drawing Link 1");
            g.drawLine((int)a1.x, (int)a1.y, (int)a2.x, (int)a2.y);
            g.fillOval((int)a2.x-3, (int)a2.y-3,6,6);
        }
        //Redraw points just in case it overlapped with arm
        g.setColor(Color.red);
        for (int i=0;i<points.size();i++) {
            g.fillOval((int)points.get(i).x-5,(int) points.get(i).y-5, 10, 10);
            //g.drawArc((int)ovals.get(i).x,(int)ovals.get(i).y,(int)ovals.get(i).size,(int)ovals.get(i).size,(int)ovals.get(i).startAngle,(int)ovals.get(i).arcAngle);
        }
        */
    }
}
